#!/usr/bin/env python
# coding=utf-8

import rospy
from socket import *
from geometry_msgs.msg import Twist, Quaternion, TransformStamped
import tf
from nav_msgs.msg import Odometry
from threading import Thread
from math import *
import time

# 轮子直径
D = 0.095
# 两个轮子之间的距离
car_width = 0.4


def callback(cmd_input, Socket):
    print("-----服务器已经启动成功，准备接收数据-----")
    Socket.settimeout(5)
    t = Twist()
    t.angular.z = cmd_input.angular.z
    t.linear.x = cmd_input.linear.x
    
    # 小车的速度策略
    left_speed = t.linear.x - t.angular.z * 0.5 * 2
    right_speed = t.linear.x + t.angular.z * 0.5 * 2
    left_speed *= 1000
    right_speed *= 1000
    left_speed = str(left_speed)
    right_speed = str(right_speed)

    print("-----发送的速度数据-----")
    print("left_speed=%s" % left_speed)
    print("right_speed=%s" % right_speed)
    
    # 将控制信息通过tcp发送给客户端
    # Socket.send(b"左轮速度")
    Socket.send(left_speed.encode("utf-8"))
    # Socket.send(b"右轮速度")
    Socket.send(right_speed.encode("utf-8"))


def Send(Socket):
    rospy.Subscriber("cmd_vel", Twist, callback, Socket)
    rospy.Rate(5)
    rospy.spin()


def Receive(Socket):
    # 发布位姿信息
    odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
    rate = rospy.Rate(1)
    # 位姿
    x = 0.0
    y = 0.0
    th = 0.0
    # 左右轮编码器初始数据
    el0 = 0
    er0 = 0
    last_time = rospy.Time.now()

    while not rospy.is_shutdown():
        # 接收来自客户端发送的编码器数据
        encode_data_left = Socket.recv(200)
        current_time = rospy.Time.now()

        if len(encode_data_left) :
            print("-----接收到编码器数据-----")
            print("recvdata:%s" % encode_data_left)
            # 对编码器数据进行处理，去除字符末尾的\n字符
               
            



            # 发布
            #odom_pub.publish(odom)

            rate.sleep()

        else:
            print('-----未接收到客户端数据，可能连接已经断开-----')
            # Socket.send(b'client off')
            # 数据中断时进行服务重启程序，先close 再accept等待重新连线
            # 可以防止出现当client意外终止导致server的中断（Broken pipe错误）
            print('-----正在重新建立连接-----')
            Socket.close()
            Socket, clientInfo = serverSocket.accept()


if __name__ == "__main__":
    rospy.init_node("base")

    serverSocket = socket(AF_INET, SOCK_STREAM)
    serverSocket.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
    serverSocket.bind(('', 8905))

    serverSocket.listen(5)
    print("-----服务器正在启动-----")
    Sockets, clientInfo = serverSocket.accept()
       
    # 将接受编码器方法作为一个子线程，接受和发送数据互不干扰
    t1 = Thread(target=Receive, args=(Sockets,))
    t1.start()
    #Send(Sockets)
